#include <iostream>
#include <sstream>
#include <vector>
#include "json.h"

using namespace std;

double calculate_distance_between_two_coordinates(double, double, double, double); // From Main.cpp
double calculate_relative_radial_position(double, double, double, double); // Main.cpp

// Map identifiers
const string Rocky_valley = "{\"size\":[200,112.500000],\"terrain\":["
"{\"boundingBox\":{\"corner\":[40,30],\"size\":[125,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[40,70],\"size\":[125,15]},\"type\":\"SOLID\"}]}";

const string Boulders = "{\"size\":[200,112.500000],\"terrain\":["
"{\"boundingBox\":{\"corner\":[30,30],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[70,30],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[110,30],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[150,30],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[30,70],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[70,70],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[110,70],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[150,70],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[30,30],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[70,30],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[110,30],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[150,30],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[30,70],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[70,70],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[110,70],\"size\":[15,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[150,70],\"size\":[15,15]},\"type\":\"SOLID\"}]}";

const string River_runs_through_it = "{\"size\":[200,112.500000],\"terrain\":["
"{\"boundingBox\":{\"corner\":[0,45],\"size\":[200,30]},\"type\":\"IMPASSABLE\"}]}";

const string Rocky_ridge = "{\"size\":[200,112.500000],\"terrain\":["
"{\"boundingBox\":{\"corner\":[30,50],\"size\":[15,90]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[90,70],\"size\":[15,100]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[90,0],\"size\":[15,30]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[150,0],\"size\":[15,50]},\"type\":\"SOLID\"}]}";

const string River_almost_runs_through_it = "{\"size\":[200,112.500000],\"terrain\":["
"{\"boundingBox\":{\"corner\":[0,45],\"size\":[80,30]},\"type\":\"IMPASSABLE\"},"
"{\"boundingBox\":{\"corner\":[120,45],\"size\":[80,30]},\"type\":\"IMPASSABLE\"}]}";

const string Pason = "{\"size\":[200,112.500000],\"terrain\":["
"{\"boundingBox\":{\"corner\":[18,60],\"size\":[5,35]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[23,90],\"size\":[10,5]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[23,75],\"size\":[10,5]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[33,80],\"size\":[5,10]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[23,80],\"size\":[10,10]},\"type\":\"IMPASSABLE\"},"
"{\"boundingBox\":{\"corner\":[59,75],\"size\":[5,10]},\"type\":\"IMPASSABLE\"},"
"{\"boundingBox\":{\"corner\":[54,65],\"size\":[5,15]},\"type\":\"IMPASSABLE\"},"
"{\"boundingBox\":{\"corner\":[64,65],\"size\":[5,15]},\"type\":\"IMPASSABLE\"},"
"{\"boundingBox\":{\"corner\":[54,60],\"size\":[15,5]},\"type\":\"IMPASSABLE\"},"
"{\"boundingBox\":{\"corner\":[49,50],\"size\":[5,15]},\"type\":\"IMPASSABLE\"},"
"{\"boundingBox\":{\"corner\":[69,50],\"size\":[5,15]},\"type\":\"IMPASSABLE\"},"
"{\"boundingBox\":{\"corner\":[59,65],\"size\":[5,10]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[93,70],\"size\":[10,5]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[88,60],\"size\":[5,10]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[103,65],\"size\":[5,5]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[93,55],\"size\":[10,5]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[103,45],\"size\":[5,10]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[93,40],\"size\":[10,5]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[88,45],\"size\":[5,5]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[126,60],\"size\":[10,5]},\"type\":\"IMPASSABLE\"},"
"{\"boundingBox\":{\"corner\":[126,30],\"size\":[10,5]},\"type\":\"IMPASSABLE\"},"
"{\"boundingBox\":{\"corner\":[121,35],\"size\":[20,5]},\"type\":\"IMPASSABLE\"},"
"{\"boundingBox\":{\"corner\":[121,55],\"size\":[20,5]},\"type\":\"IMPASSABLE\"},"
"{\"boundingBox\":{\"corner\":[121,40],\"size\":[5,15]},\"type\":\"IMPASSABLE\"},"
"{\"boundingBox\":{\"corner\":[136,40],\"size\":[5,15]},\"type\":\"IMPASSABLE\"},"
"{\"boundingBox\":{\"corner\":[126,40],\"size\":[10,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[155,20],\"size\":[5,35]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[160,40],\"size\":[5,10]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[165,30],\"size\":[5,15]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[170,25],\"size\":[5,10]},\"type\":\"SOLID\"},"
"{\"boundingBox\":{\"corner\":[175,20],\"size\":[5,35]},\"type\":\"SOLID\"}]}";

const string Great_lakes = "{\"size\":[200,112.500000],\"terrain\":[{\"boundingBox\":{\"corner\":[10,10],\"size\":[80,35]},\"type\":\"IMPASSABLE\"},{\"boundingBox\":{\"corner\":[110,10],\"size\":[80,35]},\"type\":\"IMPASSABLE\"},{\"boundingBox\":{\"corner\":[10,67.500000],\"size\":[80,35]},\"type\":\"IMPASSABLE\"},{\"boundingBox\":{\"corner\":[110,67.500000],\"size\":[80,35]},\"type\":\"IMPASSABLE\"},{\"boundingBox\":{\"corner\":[95,51.250000],\"size\":[10,10]},\"type\":\"SOLID\"}]}";

const string Rocky_cross = "{\"size\":[200,112.500000],\"terrain\":[{\"boundingBox\":{\"corner\":[10,40],\"size\":[50,5]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[10,72.500000],\"size\":[50,5]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[140,40],\"size\":[50,5]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[140,72.500000],\"size\":[50,5]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[70,15],\"size\":[5,35]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[70,62.500000],\"size\":[5,35]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[125,62.500000],\"size\":[5,35]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[125,15],\"size\":[5,35]},\"type\":\"SOLID\"}]}";

const string CC = "{\"size\":[200,112.500000],\"terrain\":[{\"boundingBox\":{\"corner\":[20,30],\"size\":[60,10]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[20,77.500000],\"size\":[60,10]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[120,30],\"size\":[60,10]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[120,77.500000],\"size\":[60,10]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[70,30],\"size\":[10,55]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[120,30],\"size\":[10,55]},\"type\":\"SOLID\"}]}";

const string Vertical_river = "{\"size\":[200,112.500000],\"terrain\":[{\"boundingBox\":{\"corner\":[80,0],\"size\":[40,115]},\"type\":\"IMPASSABLE\"},{\"boundingBox\":{\"corner\":[13,23],\"size\":[20,20]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[35,62],\"size\":[20,20]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[157,20],\"size\":[20,20]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[138,68],\"size\":[20,20]},\"type\":\"SOLID\"}]}";

const string Lake_in_the_middle = "{\"size\":[200,112.500000],\"terrain\":[{\"boundingBox\":{\"corner\":[50,42.500000],\"size\":[85,40]},\"type\":\"IMPASSABLE\"},{\"boundingBox\":{\"corner\":[140,76.500000],\"size\":[20,20]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[20,76.500000],\"size\":[20,20]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[20,22.500000],\"size\":[20,20]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[140,22.500000],\"size\":[20,20]},\"type\":\"SOLID\"}]}";

const string Random_boulders = "{\"size\":[200,112.500000],\"terrain\":[{\"boundingBox\":{\"corner\":[16,72.500000],\"size\":[20,20]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[37,26.500000],\"size\":[20,20]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[91,61.500000],\"size\":[20,20]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[145,17.500000],\"size\":[20,20]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[157,78.500000],\"size\":[20,20]},\"type\":\"SOLID\"}]}";

const string Island = "{\"size\":[200,112.500000],\"terrain\":[{\"boundingBox\":{\"corner\":[0,0],\"size\":[20,112.500000]},\"type\":\"IMPASSABLE\"},{\"boundingBox\":{\"corner\":[0,92.500000],\"size\":[200,20]},\"type\":\"IMPASSABLE\"},{\"boundingBox\":{\"corner\":[180,0],\"size\":[20,112.500000]},\"type\":\"IMPASSABLE\"},{\"boundingBox\":{\"corner\":[0,0],\"size\":[200,20]},\"type\":\"IMPASSABLE\"},{\"boundingBox\":{\"corner\":[138,31.500000],\"size\":[20,20]},\"type\":\"SOLID\"},{\"boundingBox\":{\"corner\":[40,70],\"size\":[20,20]},\"type\":\"SOLID\"}]}";

// Map paths

const vector<string> Rocky_valley_path = {
	"15 15",
	"15 58",
	"178 58",
	"178 97.5",
	"15 97.5",
	"15 58",
	"178 58",
	"178 15"
};

const vector<string> Boulders_path = {
	"15 15",
	"15 58",
	"178 58",
	"178 97.5",
	"15 97.5",
	"15 58",
	"178 58",
	"178 15"
};

const vector<string> River_runs_through_it_path = {
	"50 22",
	"150 22",
	"50 98",
	"150 98"
};

const vector<string> Rocky_ridge_path = {
	"20 20",
	"180 92.5"
};

const vector<string>River_almost_runs_through_it_path = {
	"50 22",
	"150 22",
	"50 98",
	"150 98"
};

const vector<string> Pason_path = {
	"10 55",
	"10 102",
	"35 102",
	"190 55",
	"190 12",
	"140 12"
};

const vector<string> Great_lakes_path = {
	"5 5",
	"5 50",
	"5 107.5",
	"100 107.5",
	"195 107.5",
	"195 50",
	"195 5",
	"100 5"
};

const vector<string> Rocky_cross_path = {
	"5 55",
	"100 55",
	"195 55",
	"100 55",
	"100 102.5",
	"100 55",
	"100 10",
	"100 55"
};

const vector<string> CC_path = {
	"10 15",
	"10 55",
	"10 97.5",
	"100 97.5",
	"190 97.5",
	"190 55",
	"190 15",
	"100 15"
};

const vector<string> Vertical_river_path = {
	"12 72",
	"43 25",
	"140 35",
	"165 82"
};

const vector<string> Lake_in_the_middle_path = {
	"10 10",
	"10 55",
	"10 102.5",
	"100 102.5",
	"190 102.5",
	"190 55",
	"190 10",
	"100 10"
};

const vector<string> Random_boulders_path = {
	"10 10",
	"10 55",
	"10 102.5",
	"100 102.5",
	"190 102.5",
	"190 55",
	"190 10",
	"100 10"
};

const vector<string> Island_path = {
	"40 30",
	"30 40",
	"160 82.5",
	"170 72.5"
};

string map_name = " ";
vector <vector<double>> map_features;

int our_fast_tank_path_index = -1; // Current path position we are traveling to
int our_slow_tank_path_index = -1; // Current path position we are traveling to

void read_map_features(json::Object game_state)
{
	map_features.clear();
	json::Array arr = game_state["map"]["terrain"];
	for (int i = 0; i < arr.size(); i++)
	{
		if (arr[i]["type"] == "SOLID")
		{
			vector <double> n;
			double x1 = arr[i]["boundingBox"]["corner"][(size_t)0];
			double y1 = arr[i]["boundingBox"]["corner"][(size_t)1];
			double x2 = x1 + (double)arr[i]["boundingBox"]["size"][(size_t)0];
			double y2 = y1 + (double)arr[i]["boundingBox"]["size"][(size_t)1];
			n.push_back(x1);
			n.push_back(y1);
			n.push_back(x2);
			n.push_back(y2);
			map_features.push_back(n);
		}
	}
}

string determine_current_map_name(json::Object game_state)
{
	read_map_features(game_state);
	string str = json::Serialize(game_state["map"]);
	if (str == Rocky_valley)
	{
		return "Rocky_valley";
	}
	else if (str == Boulders)
	{
		return "Boulders";
	}
	else if (str == River_runs_through_it)
	{
		return "River_runs_through_it";
	}
	else if (str == Rocky_ridge)
	{
		return "Rocky_ridge";
	}
	else if (str == River_almost_runs_through_it)
	{
		return "River_almost_runs_through_it";
	}
	else if (str == Pason)
	{
		return "Pason";
	}
	else if (str == Great_lakes)
	{
		return "Great_lakes";
	}
	else if (str == Rocky_cross)
	{
		return "Rocky_cross";
	}
	else if (str == CC)
	{
		return "CC";
	}
	else if (str == Lake_in_the_middle)
	{
		return "Lake_in_the_middle";
	}
	else if (str == Random_boulders)
	{
		return "Random_boulders";
	}
	else if (str == Vertical_river)
	{
		return "Vertical_river";
	}
	else if (str == Island)
	{
		return "Island";
	}
	else
	{
		cout << str << endl;
		return "  ";
	}
}

void set_current_map_name(string name)
{
	map_name = name;
}

string get_current_map_name()
{
	return map_name;
}

int determine_nearest_path_index(double x, double y)
{
	// Variables
	int nearest_index = 0;
	vector<string> map_path;

	string map_name = get_current_map_name();

	if (map_name == "River_almost_runs_through_it")
	{
		map_path = River_almost_runs_through_it_path;
	}
	else if (map_name == "Rocky_valley")
	{
		map_path = Rocky_valley_path;
	}
	else if (map_name == "Boulders")
	{
		map_path = Boulders_path;
	}
	else if (map_name == "River_runs_through_it")
	{
		map_path = River_runs_through_it_path;
	}
	else if (map_name == "Rocky_ridge")
	{
		map_path = Rocky_ridge_path;
	}
	else if (map_name == "Pason")
	{
		map_path = Pason_path;
	}
	else if (map_name == "Great_lakes")
	{
		map_path = Great_lakes_path;
	}
	else if (map_name == "Rocky_cross")
	{
		map_path = Rocky_cross_path;
	}
	else if (map_name == "CC")
	{
		map_path = CC_path;
	}
	else if (map_name == "Lake_in_the_middle")
	{
		map_path = Lake_in_the_middle_path;
	}
	else if (map_name == "Random_boulders")
	{
		map_path = Random_boulders_path;
	}
	else if (map_name == "Vertical_river")
	{
		map_path = Vertical_river_path;
	}
	else if (map_name == "Island")
	{
		map_path = Island_path;
	}

	for (int test_index = 1; test_index < map_path.size(); test_index++)
	{
		stringstream coordinates(map_path[test_index]);
		double path1_x, path1_y;
		coordinates >> path1_x >> path1_y;

		coordinates = stringstream(map_path[nearest_index]);
		double path2_x, path2_y;
		coordinates >> path2_x >> path2_y;

		double distance1 = calculate_distance_between_two_coordinates(x, y, path1_x, path1_y);
		double distance2 = calculate_distance_between_two_coordinates(x, y, path2_x, path2_y);

		if (distance1 < distance2)
		{
			nearest_index = test_index;
		}
	}
	return nearest_index;
}

stringstream get_path_coordinate(int& point_index)
{
	string map_name = get_current_map_name();

	if (map_name == "River_almost_runs_through_it")
	{
		if (point_index == River_almost_runs_through_it_path.size())
		{
			point_index = 0;
		}
		return stringstream(River_almost_runs_through_it_path[point_index]);
	}
	else if (map_name == "Rocky_valley")
	{
		if (point_index == Rocky_valley_path.size())
		{
			point_index = 0;
		}
		return stringstream(Rocky_valley_path[point_index]);
	}
	else if (map_name == "Boulders")
	{
		if (point_index == Boulders_path.size())
		{
			point_index = 0;
		}
		return stringstream(Boulders_path[point_index]);
	}
	else if (map_name == "River_runs_through_it")
	{
		if (point_index == River_runs_through_it_path.size())
		{
			point_index = 0;
		}
		return stringstream(River_runs_through_it_path[point_index]);
	}
	else if (map_name == "Rocky_ridge")
	{
		if (point_index == Rocky_ridge_path.size())
		{
			point_index = 0;
		}
		return stringstream(Rocky_ridge_path[point_index]);
	}
	else if (map_name == "Pason")
	{
		if (point_index == Pason_path.size())
		{
			point_index = 0;
		}
		return stringstream(Pason_path[point_index]);
	}
	else if (map_name == "Great_lakes")
	{
		if (point_index == Great_lakes_path.size())
		{
			point_index = 0;
		}
		return stringstream(Great_lakes_path[point_index]);
	}
	else if (map_name == "Rocky_cross")
	{
		if (point_index == Rocky_cross_path.size())
		{
			point_index = 0;
		}
		return stringstream(Rocky_cross_path[point_index]);
	}
	else if (map_name == "CC")
	{
		if (point_index == CC_path.size())
		{
			point_index = 0;
		}
		return stringstream(CC_path[point_index]);
	}
	else if (map_name == "Lake_in_the_middle")
	{
		if (point_index == Lake_in_the_middle_path.size())
		{
			point_index = 0;
		}
		return stringstream(Lake_in_the_middle_path[point_index]);
	}
	else if (map_name == "Random_boulders")
	{
		if (point_index == Random_boulders_path.size())
		{
			point_index = 0;
		}
		return stringstream(Random_boulders_path[point_index]);
	}
	else if (map_name == "Vertical_river")
	{
		if (point_index == Vertical_river_path.size())
		{
			point_index = 0;
		}
		return stringstream(Vertical_river_path[point_index]);
	}
	else if (map_name == "Island")
	{
		if (point_index == Island_path.size())
		{
			point_index = 0;
		}
		return stringstream(Island_path[point_index]);
	}
}

bool is_trajectory_blocked(double x1, double y1, double x2, double y2)
{
	double direction = calculate_relative_radial_position(x1, y1, x2, y2) / 180 * 3.14159265359;
	double d = calculate_distance_between_two_coordinates(x1, y1, x2, y2) / 200;
	for (int n = 0; n <= 200; n++)
	{
		double x = x1 + cos(direction) * d * n;
		double y = y1 + sin(direction) * d * n;
		for (vector <double> a : map_features)
		{
			double x1 = a[0];
			double y1 = a[1];
			double x2 = a[2];
			double y2 = a[3];
			if (x >= x1 && x <= x2 && y >= y1 && y <= y2)
			{
				return true;
			}
		}
	}
	return false;
}

int get_our_fast_tank_path_index()
{
	return our_fast_tank_path_index;
}

void set_our_fast_tank_path_index(int a)
{
	our_fast_tank_path_index = a;
}

int get_our_slow_tank_path_index()
{
	return our_slow_tank_path_index;
}

void set_our_slow_tank_path_index(int a)
{
	our_slow_tank_path_index = a;
}
